
/*************
author:: libo
license:: MIT
*****************/

#include<tool/libcell_tools_algorithm.h>

#include<Mesh/libcell_iterator.h>
#include <Math/LB_Math.h>
#include <Math/LB_Quaternions.h>
#include <Matrix/LB_Matrix.h>

#define quote lib_cell_quote

double* mesh_compute_bounding_box_from_node(Node* n)
{
    if(n==NULL)
    {
        return NULL;
    }
    double* re=(double*)malloc(sizeof(double)*6);
    template_v* v=(template_v*)(n->value);
    re[0]=v->point[0];re[1]=v->point[1];re[2]=v->point[2];
    re[3]=re[0];re[4]=re[1];re[5]=re[2];
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v=(template_v*)(nit->value);
        for(int i=0;i<3;i++)
        {
            v->point[i]>re[3+i]?(re[3+i]=v->point[i]):(re[i+3]=re[i+3]);
            v->point[i]<re[i]?(re[i]=v->point[i]):(re[i]=re[i]);
        }
    }
    return re; 
}
double* mesh_compute_bounding_box(Mesh* m)
{
    double * re=(double*)malloc(sizeof(double)*6);
    auto it=m->v_begin(m);
    template_v* v=quote(it);
    re[0]=v->point[0];re[1]=v->point[1];re[2]=v->point[2];
    re[3]=re[0];re[4]=re[1];re[5]=re[2];
    it++;

    for(;it!=m->v_end(m);it++)
    {
        v=quote(it);
        for(int i=0;i<3;i++)
        {
            v->point[i]>re[3+i]?(re[3+i]=v->point[i]):(re[i+3]=re[i+3]);
            v->point[i]<re[i]?(re[i]=v->point[i]):(re[i]=re[i]);
        } 


    }
    return re;

}
void mesh_adjust_mesh_to_unit_hexahedron(Mesh* m)
{

    Node* n=NULL;
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        n=node_overlying(n,quote(vit));
    }
    double* box=mesh_compute_bounding_box_from_node(n);
    if(box==NULL)
    {
        free_node(n);
        return ;
    }
    printf("box:%lf %lf %lf %lf %lf %lf\n",box[0],box[1],box[2],box[3],box[4],box[5]);
    double centroid[3]={(box[0]+box[3])/2.0,(box[1]+box[4])/2.0,(box[2]+box[5])/2.0};
    printf("centroid : %lf %lf %lf\n",centroid[0],centroid[1],centroid[2]);
    double max=box[3]-box[0];

    max=((box[4]-box[1])>max?((box[4]-box[1])):(max));
    max=((box[5]-box[2])>max?((box[5]-box[2])):(max));   
    printf("max:%lf\n",max);
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        template_v*v=(template_v*)(quote(vit));
        v->point[0]-=centroid[0];
        v->point[1]-=centroid[1];
        v->point[2]-=centroid[2];
        v->point[0]*= (2.0/max);
        v->point[1]*= (2.0/max);
        v->point[2]*= (2.0/max);
 
    }

    free(box); 
    free_node(n);
    
    
}

Node* Mesh_adjust_halffaces(Mesh* m,template_c* c)
{
     Node* hfs=node_copy(c->halffaces);
     Node* re=NULL;
     re=node_pushback(re,hfs->value);
     hfs=node_remove(hfs,hfs);
     int flag=1;
     while(flag)
     {
        template_hf* hf=(template_hf*)(re->value);
        if(hf->vertices_size<=1)
        {
            free_node(hfs);
            return re;
        }
        template_v* v=hf->vertices[1];
        flag=0;
        for(Node* nit=hfs;nit!=NULL;nit=(Node*)(nit->Next))
        { 
            template_hf* hf1=(template_hf*)(nit->value);
            if(Mesh_vertex_is_in_face(m,v,hf1->face))
            {
                flag=1;
                re=node_pushback(re,nit->value);
                hfs=node_remove(hfs,nit);
                break;
            }
        }
     }
     free_node(hfs);
     return node_reverse(re);
     
}
//modified 
static Node* my_get_cut_vertices_node(Mesh*m,Node* faces)
{
    //printf("begin generate node2\n");
    Node* node1=NULL,*node2=NULL;
    for(Node* nit=faces;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_f* f=(template_f*)(nit->value);
        for(int i=0;i<2;i++)
        {
          //  printf("v id%d\n",f->vertices[i]->id ); 
            if(node_find(node2,f->vertices[i])==NULL)
            {
                if(node_find(node1,f->vertices[i])==NULL)
                {
               //     printf("one\n");
                    node1=node_overlying(node1,f->vertices[i]);
                }
                else
                {
               //     printf("two\n" );
                    node1=node_delete_value(node1,f->vertices[i]);
                    node2=node_overlying(node2,f->vertices[i]);
                }
            }
            
        }
    }
    Node* node=node_copy(node1);
  //  printf("node 1 size:%d node 2 size:%d\n",node_size(node1),node_size(node2) );
    for(Node* nit=node;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v*v=(template_v*)(nit->value);
        if(m->vertex_is_boundary(m,*v))
        {
   //         printf("boudnary\n");
            node2=node_overlying(node2,v);
            //node1=node_delete_value(node1,v);
        }
    }
    free_node(node);
    free_node(node1);
    return node2;
}
//tree1储存 新建cell到旧cell的映射
//
static inline void my_cut_oneside(Mesh* m,Node* node2,Int_RB_Tree* tree,Int_RB_Tree* tree1,Int_RB_Tree* tr1)
{
    // Int_RB_Tree* tr1=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    // int_rb_tree_init(tr1);
    Int_RB_Tree* tr2=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tr2);
    for(Node* nit=node2;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        template_v* v1=m->create_vertexv(m,v->point,3);
        tr1->insert(tr1,v1->id,v);
        tr2->insert(tr2,v->id,v1); 
    }
    Node*node_c=NULL,*node_cids=NULL;
    for(auto it=tree->begin(tree);it.it!=NULL;it++)
    {
        template_c* c=(template_c*)(*it);
        Node* hfs_n=NULL;
        for(auto it1=m->chf_begin(m,*c);it1!=m->chf_end(m,*c);it1++)
        {
            Node* hf_n=NULL; 
            for(int i=0;i<2;i++)
            {
                template_v* v=quote(it1)->vertices[i];
                template_v* v1=(template_v*)(tr2->find(tr2,v->id));
                v1!=NULL?(hf_n=node_pushback(hf_n,v1)):(hf_n=node_pushback(hf_n,v));
            }
            hfs_n=node_overlying(hfs_n,node_reverse(hf_n));           
        }
        node_c=node_overlying(node_c,hfs_n); 
        int* cid_value=(int*)malloc(sizeof(int));
        *cid_value=c->id;
        node_cids=node_overlying(node_cids,cid_value);
        m->delete_cell(m,*c,true);
    }
    Node*nit11=node_cids;
    for(Node*nit=node_c;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Node* n1=(Node*)(nit->value);
        int size1=node_size(n1);
        template_hf**temp_hf=(template_hf**)malloc(sizeof(template_hf*)*size1);
        template_v**temp_v=NULL;
        int j=0;
        for(Node* nit1=n1;nit1!=NULL;nit1=(Node*)(nit1->Next))
        {
            Node* n2=(Node*)(nit1->value);
            int size=node_size(n2);
            temp_v=(template_v**)malloc(sizeof(template_v*)*size);
            int i=0;
            for(Node* nit2=n2;nit2!=NULL;nit2=(Node*)(nit2->Next))
            {
                temp_v[i]=(template_v*)(nit2->value);
                i++;
            }
            template_f* f=m->create_facev(m,temp_v,size);
            temp_hf[j]=m->create_halfface(m,f,temp_v,size);
            free(temp_v);
            j++;
        }
        template_c* c=m->create_cellf(m,temp_hf,size1);
        tree1->insert(tree1,c->id,nit11->value);
        nit11=(Node*)(nit11->Next);
        free(temp_hf);
    }  
    free_node(node_cids);
    for(Node* nit=node_c;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Node* n1=(Node*)(nit->value);
        for(Node* nit1=n1;nit1!=NULL;nit1=(Node*)(nit1->Next))
        {
            Node* n2=(Node*)(nit1->value);
            free_node(n2);
        }
        free_node(n1);
    }
    free_node(node_c);
    int_rb_tree_free(tr2);
}
//
// 这个函数需要修改，因为对于某些情况并不能找到完整的connected cells
//

static Int_RB_Tree* my_get_connected_cellsn(Mesh* m,Int_RB_Tree* tree,Node* faces)
{
    Int_RB_Tree*re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);

    return re;
}
static Int_RB_Tree* my_get_connected_cells(Mesh* m,Int_RB_Tree* tree,Node* faces)
{
    Int_RB_Tree*re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);
    auto it1=tree->begin(tree);
    template_c* c=(template_c*)(*it1);
    if(c==NULL)
    {
        return re;
    }  
    Node* tempn=NULL;
    tempn=node_overlying(tempn,c);
    tree->erase(tree,c->id);
    re->insert(re,c->id,c);
    while(tempn!=NULL)
    {
        Node* tempn1=NULL;
        for(Node* nit=tempn;nit!=NULL;nit=(Node*)(nit->Next))
        {
            template_c* c2=(template_c*)(nit->value);
            for(auto it=m->chf_begin(m,*c2);it!=m->chf_end(m,*c2);it++)
            {
                if(node_find(faces,quote(it)->face)!=NULL)
                {
                    continue;
                }
                template_c* c3=m->s_opposite_halfface(quote(it))->cell;
                if(c3==NULL||tree->find(tree,c3->id)==NULL)
                {
                    continue;
                }
                tempn1=node_overlying(tempn1,c3);
             
                tree->erase(tree,c3->id);
                re->insert(re,c3->id,c3);
            }            
        }
        free_node(tempn);
        tempn=tempn1;        
    } 


    return re;
}
void Mesh_cut_along_the_curvef(Mesh* m,Node* faces,Int_RB_Tree* re,Int_RB_Tree* re1)
{
    //printf("mesh cut along_the curve f node :%d\n",node_size(faces) );

    m->manifold_require=1;
   // printf("face size:%d\n",node_size(faces) );
    Node* node2=my_get_cut_vertices_node(m,faces);
    //printf("node2 size:%d\n",node_size(node2) ); 
    Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree); 
    for(Node* nit=node2;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        for(Node*nit1=v->cells;nit1!=NULL;nit1=(Node*)(nit1->Next))
        {
            template_c* c=(template_c*)(nit1->value);
            tree->insert(tree,c->id,c);  
        } 
    }
    //printf("tree size:%d\n",tree->size);
    while(tree->size>0)
    {
        //printf("tree size:%d\n",tree->size);

        Int_RB_Tree* tree1=my_get_connected_cells(m,tree,faces);
        my_cut_oneside(m,node2,tree1,re,re1);
        int_rb_tree_free(tree1);
    }
    int_rb_tree_free(tree);
    free_node(node2);
    return ;
}
void Mesh_cut_along_the_curvev(Mesh* m,Node* vertices,Int_RB_Tree* tree,Int_RB_Tree* tree1)
{   
    Node* edges=NULL;
    template_v* vs[2];
    for(Node* nit=vertices;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        Node* nit1=(Node*)(nit->Next);
        template_v* v1=(template_v*)(vertices->value);
        if(nit1!=NULL)
        {
            v1=(template_v*)(nit1->value);
        }
        vs[0]=v;vs[1]=v1;
        template_f* f=m->get_facev(m,vs,2);
        if(f==NULL)
        {

            printf("cuowu\n");
            continue;
        }
        edges=node_overlying(edges,f);
    } 
    Mesh_cut_along_the_curvef(m,edges,tree,tree1);

    free_node(edges);

    return ;
}
Node* split_unicom_region_from_sub_node( Node*node )
{
    Node* re=NULL;
    Int_RB_Tree* tree0=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree0);
    Int_RB_Tree* tree1=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree1);
    for(Node* nit=node;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c* c=(template_c*)(nit->value);
        tree0->insert(tree0,c->id,c);tree1->insert(tree1,c->id,c);
    }

    while(tree0->size>0)
    {
        template_c* c=(template_c* )(tree0->begin(tree0).second);
        Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
        int_rb_tree_init(tree);
        tree->insert(tree,c->id,c);
        tree0->erase(tree0,c->id);
        //*****************
        Node* temp=NULL;
        temp=node_overlying(temp,c);
        
        Node* temp1=NULL;
        while(temp!=NULL)
        {
            for(Node* nit=temp;nit!=NULL;nit=(Node*)(nit->Next))
            {
                c=(template_c*)(nit->value);
                for(Node*nit1=c->halffaces;nit1!=NULL;nit1=(Node*)(nit1->Next) )
                {
                    template_c* c3=Mesh_s_opposite_halfface(((template_hf*)(nit1->value)))->cell;
                    if(c3==NULL||tree->find(tree,c3->id)!=NULL||tree1->find(tree1,c3->id)==NULL )
                    {
                        continue;
                    }
                    else
                    {
                        tree->insert(tree,c3->id,c3);
                        tree0->erase(tree0,c3->id);
                        temp1=node_overlying(temp1,c3);
                    }
                }
            }
            free_node(temp);
            temp=temp1;
            temp1=NULL;
        }
        printf("%d\n",tree->size);
        //******************
        re=node_overlying(re,tree);
    }

    int_rb_tree_free(tree0);
    int_rb_tree_free(tree1);

    return re;

}
Node* split_unicom_region( Mesh * m )
{

    Node* re=NULL;
    Int_RB_Tree* tree0=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree0);
    for(auto cit=m->c_begin(m);cit!=m->c_end(m);cit++)
    {
        tree0->insert(tree0,quote(cit)->id,quote(cit));
    }

    //printf("num c:%d %d\n",tree0->size,m->num_c(m));
    while(tree0->size>0)
    {
        template_c* c=(template_c* )(tree0->begin(tree0).second);
        Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
        int_rb_tree_init(tree);
        tree->insert(tree,c->id,c);
        tree0->erase(tree0,c->id);
        //*****************
        Node* temp=NULL;
        temp=node_overlying(temp,c);
        
        Node* temp1=NULL;
        while(temp!=NULL)
        {
            for(Node* nit=temp;nit!=NULL;nit=(Node*)(nit->Next))
            {
                c=(template_c*)(nit->value);
                for(auto chfit=m->chf_begin(m,*c); chfit!=m->chf_end(m,*c);chfit++)
                {
                    template_c* c3=m->s_opposite_halfface(quote(chfit))->cell;
                    if(c3==NULL||tree->find(tree,c3->id)!=NULL)
                    {
                        continue;
                    }
                    else
                    {
                        tree->insert(tree,c3->id,c3);
                        tree0->erase(tree0,c3->id);
                        temp1=node_overlying(temp1,c3);
                    }
                }
            }
            free_node(temp);
            temp=temp1;
            temp1=NULL;
        }
        printf("%d\n",tree->size);
        //******************
        re=node_overlying(re,tree);
    }

    int_rb_tree_free(tree0);
    return re;
}

Node* split_unicom_region2mesh(Mesh *m )
{
    Node* re=NULL;
    Node* node=split_unicom_region(m);
    
    for(Node* nit=node;nit!=NULL;nit=(Node*)(nit->Next))
    {   
        Int_RB_Tree* tree=(Int_RB_Tree*)(nit->value);
        Mesh * mesh=(Mesh*)malloc(sizeof(Mesh));
        Mesh_init(mesh);
        mesh->dimension=m->dimension;
        mesh->simplex=m->simplex;
        mesh->manifold_require=m->manifold_require;
//*********************

        Int_RB_Tree* treev=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
        int_rb_tree_init(treev);

        for(auto it=tree->begin(tree);it.it!=NULL;it++)
        {
            template_c* c=(template_c*)(it.second);
            for(int j=0;j<c->vertices_size;j++)
            {
                if(treev->find(treev,c->vertices[j]->id)==NULL)
                {
                    template_v*v= mesh->create_vertexv(mesh,c->vertices[j]->point,c->vertices[j]->point_size);
                    
                    treev->insert(treev,c->vertices[j]->id,v);
                }
            }

            int j=0;
            int hfs_l=node_size(c->halffaces);
            template_hf**hfs=(template_hf**)malloc(sizeof(template_hf*)*hfs_l);
            for(auto chfit=m->chf_begin(m,*c);chfit!=m->chf_end(m,*c);chfit++)
            {
                template_v**vs=(template_v**)malloc(sizeof(template_v*)*quote(chfit)->vertices_size);
                for(int i=0;i<quote(chfit)->vertices_size;i++)
                {
                    vs[i]=(template_v*)(treev->find(treev,quote(chfit)->vertices[i]->id));
                }

                template_f*f=mesh->create_facev(mesh, vs,quote(chfit)->vertices_size );
                hfs[j]=mesh->create_halfface(mesh,f,vs,quote(chfit)->vertices_size);

                j++;
                free(vs);
            }

            mesh->create_cellf(mesh,hfs,hfs_l);
            free(hfs);
        }
    

        int_rb_tree_free(treev);
//*******************
        re=node_overlying(re,mesh);

        int_rb_tree_free(tree);
    }

    free_node(node);
   
    return re;
}

static template_v* get_next_shortest_vertex(Mesh*m,Int_RB_Tree* tree,Int_RB_Tree* tree1,Node* vs)
{
    template_v* v=(template_v*)(vs->value);
    for(auto vfit=m->vf_begin(m,*v);vfit!=m->vf_end(m,*v);vfit++)
    {
        template_v* v1=NULL;
        (quote(vfit)->vertices[0]==v)?(v1=quote(vfit)->vertices[1]):(v1=quote(vfit)->vertices[0]);
        
        double* value=(double*)(tree->find(tree,v1->id));
        double* value1=(double*)(tree->find(tree,v->id));
        if(value==NULL)
        {
            value=(double*)malloc(sizeof(double));
            *value=*value1+ distance_of_two_points(v->point,v1->point,3);
            tree->insert(tree,v1->id,value);
            //printf("inset:%d %lf\n",v1->id,*value);
            tree1->insert(tree1,v1->id,value);
        }
        else
        {
            if(*value>*value1+distance_of_two_points(v->point,v1->point,3))
            {
                *value=*value1+ distance_of_two_points(v->point,v1->point,3);
            }
        }
        //printf("once\n");

    }
    //printf("here\n");

    //printf("tree1->size:%d\n",tree1->size);
    auto it=tree1->begin(tree1);
    int id=it.first;
    double temp_value=*((double*)(it.second));
    
    for(;it.it!=NULL;it++)
    {
        if(*((double*)(it.second))<temp_value)
        {
            id=it.first;
            temp_value=*((double*)(it.second));
        }
    }
    //printf("here1 %lf %d\n",temp_value,id);

    return m->get_vertexp(m,id);
} 
static Node* get_shortest_path1(Mesh* m, template_v* v1,template_v*v2,Int_RB_Tree* tree)
{
    Node* re=NULL;
    template_v* v=v2;
    while(v!=v1)
    {
        re=node_overlying(re,v);
        //printf("ud:%d\n",v->id);
        template_v* v3=NULL,*v4=NULL;
        double temp=0;

        for(auto vfit=m->vf_begin(m,*v);vfit!=m->vf_end(m,*v);vfit++)
        {
            (quote(vfit)->vertices[0]==v)?(v3=quote(vfit)->vertices[1]):(v3=quote(vfit)->vertices[0]);
            double *value=(double*)(tree->find(tree,v3->id));
            if(value==NULL)
            {
                continue;
            }
            
            if(*value<temp||v4==NULL)
            {
                temp=*value;
                v4=v3;
            }
        }
        v=v4;
    }
    re=node_overlying(re,v1);

    return re;
}

Node* get_shortest_path(Mesh* m, template_v* v1,template_v*v2)
{

    Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree);

    Int_RB_Tree* tree1=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree1);
    Node* vs=NULL;
    vs=node_overlying(vs,v1);

    double*value=(double*)malloc(sizeof(double));
    *value=0;
    tree->insert(tree,v1->id,value);

    //printf("begin shortest \n");
    template_v *v=NULL;
    //int nums=0;
    do
    {
        v =get_next_shortest_vertex(m,tree,tree1,vs);
        vs=node_overlying(vs,v);
        tree1->erase(tree1,v->id);
        //printf("erase:%d %d\n",v->id,v2->id);
        // if(nums>60)
        // {
        //     break;
        // }
        // nums++;
    }while(v!=v2);
    //printf("begin shortest \n");

    Node* re1=get_shortest_path1(m, v1,v2,tree);

    //printf("end shortest \n");

    int_rb_tree_free_value(tree);free_node(vs);
    int_rb_tree_free(tree);int_rb_tree_free(tree1);

    return re1;
}
static inline double compute_triangle_area(double* p1,double* p2,double* p3)
{
    double q1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    
    double q2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};

    double* q=out_product(q1,q2);

    double re=SAFE_SQRT(q[0]*q[0]+q[1]*q[1]+q[2]*q[2])/2.0;

    SAFE_FREE(q);

    return re;

}
double compute_surface_area(Mesh *m)
{
    double re=0;
    for(auto cit=m->c_begin(m);cit!=m->c_end(m);cit++)
    {
        template_v* v=quote(cit)->vertices[0];
        for(auto chfit=m->chf_begin(m,*cit);chfit!=m->chf_end(m,*cit);chfit++)
        {
            if(quote(chfit)->vertices[0]!=v&&quote(chfit)->vertices[1]!=v )
            {

                re+=compute_triangle_area(v->point,quote(chfit)->vertices[0]->point,
                    quote(chfit)->vertices[1]->point );
            }
        }
    }
    
    return re;
}

static inline void  get_cube_from_cell(double *p1,double* p2,double* p3,double *cube)
{
    memset(cube,0,sizeof(double)*4);
    cube[0] = p1[0];    cube[1] = p1[1];
    cube[2] = p1[0];    cube[3] = p1[1];
    p2[0]<cube[0]?(cube[0]=p2[0]):(p2[0]>cube[2]?(cube[2]=p2[0]):(cube[2]=cube[2]));
    p2[1]<cube[1]?(cube[1]=p2[1]):(p2[1]>cube[3]?(cube[3]=p2[1]):(cube[3]=cube[3]));

    p3[0]<cube[0]?(cube[0]=p3[0]):(p3[0]>cube[2]?(cube[2]=p3[0]):(cube[2]=cube[2]));
    p3[1]<cube[1]?(cube[1]=p3[1]):(p3[1]>cube[3]?(cube[3]=p3[1]):(cube[3]=cube[3]));
    
}
static inline void fill_center_data(template_c* c,double* cen)
{
    double min[3]={c->vertices[0]->point[0],c->vertices[0]->point[1],c->vertices[0]->point[2]};
    double max[3]={min[0],min[1],min[2]};
    c->vertices[1]->point[0]<min[0]?(min[0]=c->vertices[1]->point[0]):(
                c->vertices[1]->point[0]>max[0]?(max[0]=c->vertices[1]->point[0]):(max[0]=max[0]));
    
    c->vertices[1]->point[1]<min[1]?(min[1]=c->vertices[1]->point[1]):(
                c->vertices[1]->point[1]>max[1]?(max[1]=c->vertices[1]->point[1]):(max[1]=max[1]));

    c->vertices[1]->point[2]<min[2]?(min[2]=c->vertices[1]->point[2]):(
                c->vertices[1]->point[2]>max[2]?(max[2]=c->vertices[1]->point[2]):(max[2]=max[2]));


    c->vertices[2]->point[0]<min[0]?(min[0]=c->vertices[2]->point[0]):(
                c->vertices[2]->point[0]>max[0]?(max[0]=c->vertices[2]->point[0]):(max[0]=max[0]));
    
    c->vertices[2]->point[1]<min[1]?(min[1]=c->vertices[2]->point[1]):(
                c->vertices[2]->point[1]>max[1]?(max[1]=c->vertices[2]->point[1]):(max[1]=max[1]));

    c->vertices[2]->point[2]<min[2]?(min[2]=c->vertices[2]->point[2]):(
                c->vertices[2]->point[2]>max[2]?(max[2]=c->vertices[2]->point[2]):(max[2]=max[2]));

    cen[0]=(min[0]+max[0])/2.0;cen[1]=(min[1]+max[1])/2.0;cen[2]=(min[2]+max[2])/2.0;
}
static void sampling_from_one_cell(template_c*c,Node**n,double dense )
{
    double cen[3]={0};
    fill_center_data(c,cen);
    // for(int i=0;i<3;i++)
    // {
    //     for(int j=0;j<3;j++)
    //     {
    //         cen[i]+=c->vertices[j]->point[i];
    //     }
    // }
    // cen[0]/=3.0;cen[1]/=3.0;cen[2]/=3.0;
    double p1[3]={c->vertices[0]->point[0]-cen[0],c->vertices[0]->point[1]-cen[1],
        c->vertices[0]->point[2]-cen[2]};

    double p2[3]={c->vertices[1]->point[0]-cen[0],c->vertices[1]->point[1]-cen[1],
        c->vertices[1]->point[2]-cen[2]};

    double p3[3]={c->vertices[2]->point[0]-cen[0],c->vertices[2]->point[1]-cen[1],
        c->vertices[2]->point[2]-cen[2]};

    double q1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};

    double q2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};

    double *normal=out_product(q1,q2);
    if(!normalize(normal,3))
    {
        SAFE_FREE(normal);
        return ;
    }
    double theta = acos(normal[2]);

    double  n1[3]={normal[1],-normal[0],0};

    if(normalize(n1,3))
    {
        double*matd = get_matrix_from_rotation(theta,n1);
        double pp1[3]={0},pp2[3]={0},pp3[3]={0};
        for(int i=0;i<3;i++)
        {
            for(int j=0;j<3;j++)
            {
                pp1[i]+=(matd[i*3+j]*p1[j]);
                pp2[i]+=(matd[i*3+j]*p2[j]);
                pp3[i]+=(matd[i*3+j]*p3[j]);
            }
        }
        memmove(p1,pp1,sizeof(double)*3);
        memmove(p2,pp2,sizeof(double)*3);
        memmove(p3,pp3,sizeof(double)*3);
    }

    double cube[4]={0};
    get_cube_from_cell(p1,p2,p3,cube);
    int min_x=(int)(floor(cube[0]/dense));
    int min_y=(int)(floor(cube[1]/dense));

    int max_x=(int)(ceil(cube[2]/dense));
    int max_y=(int)(ceil(cube[3]/dense));
    LB_Matrix* mat=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(mat,2,2);
    double* data=(double*)(mat->data);
    data[0]=p2[0]-p1[0]; data[1]=p3[0]-p1[0];
    data[2]=p2[1]-p1[1]; data[3]=p3[1]-p1[1];

    LB_Matrix* mati=mat->inverse(mat);
    data=(double*)(mati->data);
    double y[3]={0},pp[2]={0};
    for(int i=min_x;i<max_x;i++)
    {
        pp[0]=(i+(rand()%1000)/1000.0)*dense-p1[0];
        
        for(int j=min_y;j<max_y;j++)
        {
            pp[1]=(j+(rand()%1000)/1000.0)*dense-p1[1];
            y[1]=pp[0]*data[0]+pp[1]*data[1];
            y[2]=pp[0]*data[2]+pp[1]*data[3];
            y[0]=1-y[1]-y[2];
            if(y[0]>0&&y[1]>0&&y[2]>0)
            {
                Vertex*v=(Vertex*)malloc(sizeof(Vertex));
                Vertex_init_(v);
                v->point_size=3;
                v->point=(double*)malloc(sizeof(double)*3);
                v->point[0]=y[0]*c->vertices[0]->point[0]+y[1]*c->vertices[1]->point[0]+y[2]*c->vertices[2]->point[0];
                v->point[1]=y[0]*c->vertices[0]->point[1]+y[1]*c->vertices[1]->point[1]+y[2]*c->vertices[2]->point[1];
                v->point[2]=y[0]*c->vertices[0]->point[2]+y[1]*c->vertices[1]->point[2]+y[2]*c->vertices[2]->point[2];
                v->prop=c;
                *n =node_overlying(*n,v);
            }
        }
    } 
    lb_matrix_free(mat);
    lb_matrix_free(mati);
}
Node* create_surface_uniform_sampling_points(Mesh* m,int num,double dense)
{
    Node* re=NULL;
    srand(25);

    if(num>0)
    {
        double area=compute_surface_area(m);
        dense=SAFE_SQRT(area/(double)num);
    }

    for(auto cit=m->c_begin(m);cit!=m->c_end(m);cit++)
    {
        sampling_from_one_cell(quote(cit),&re,dense );
    }

    return re;
}


